/**
 * Main
 *
 * @author Jasper Nevens <jasper@nevensit.be>
 * @date    18/02/11
 */

#include <iostream>
#include <stdio.h>
#include <string>
#include <math.h>
#include "motors.h"
#include "muis.h"
#include "movement.h"

using namespace std;

int main(int argc, char *argv[])
{
	int compoort;
	printf("Op welke compoort zit de motorsturing? poort: ");
	scanf("%d%*c",&compoort);
	Motors* motors=new Motors(compoort,19200);
	// be sure motors had stopped
	motors->stopMotors();
	//robotje.initHardware();
	
	char in;
	int speed = 50;
	
	printf("Snelheid veranderen met h\n");
	printf("Richting met wasd\n");
	scanf("%d%*c",&speed);
	if(speed < 0 || speed > 100)
		speed = 50;
	
	while(1)
	{
		in=getchar();
		//in = getche();
		switch(in)
		{
			case 'w' :
				motors->setSpeed(speed,MOTOR_FORWARD,speed,MOTOR_FORWARD);
				break;
			case 's' :
				motors->setSpeed(speed,MOTOR_REVERSE,speed,MOTOR_REVERSE);
				break;
			case 'd' :
				motors->setSpeed(speed,MOTOR_REVERSE,speed,MOTOR_FORWARD);
				break;
			case 'a' :
				motors->setSpeed(speed,MOTOR_FORWARD,speed,MOTOR_REVERSE);
				break;
			case 'h' :
				printf("Snelheid [0-100]: ");
				scanf("%d%*c",&speed);
				if(speed < 0 || speed > 100)
					speed = 50;
				break;	
		}
		
		usleep(500000);
		motors->setSpeed(0,MOTOR_BRAKE,0,MOTOR_BRAKE);
	}
} 

